/*
 * Funcs.h
 *
 *  Created on: Apr 3, 2013
 *      Author: root
 */

#include "Buffer.h"

#define RED    0
#define BLUE   1
#define YELLOW 2
#define ORANGE 3
#define BROWN  4
#define GREEN  5

#define AIR    0
#define LAND   1
#define RAIL   2


void AllocateImage(IplImage** image,CvSize size,int size2,int size3){
	if(*image == NULL){
		*image = cvCreateImage(size, size2, size3);
	}
}

int* AutoRange(CvCapture* cap){

	int color = 0;
	double area[6];
	int* Block = new int[2];
	for(int x=0;x<6;x++)
		area[x] = 0;
	Block[0] = -1;
	Block[1] = -1;
	int** Mean = new int*[10];
	for(int x=0;x<10;x++){
		Mean[x] = new int[2];
	}
	for(int x=0;x<10;x++){
		Mean[x][0] = -1;
		Mean[x][1] = -1;
		}
for(int i=0;i<10;i++){
	IplImage* img = cvQueryFrame(cap);

	IplImage* imgHSV =  cvCreateImage(cvGetSize(img),8,3);
	IplImage* imgHSV2 = cvCreateImage(cvGetSize(img),8,1);

	cvCvtColor(img, imgHSV, CV_BGR2HSV);

	CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));

	for(color=0;color<6;color++){

		if(color == RED){
			cvInRangeS(imgHSV, cvScalar(0/2, 100, 150), cvScalar(5/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[0] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == BLUE){
			cvInRangeS(imgHSV, cvScalar(210/2, 152, 0), cvScalar(215/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[1] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == YELLOW){
			cvInRangeS(imgHSV, cvScalar(30/2, 100, 100), cvScalar(60/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[2] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == ORANGE){
			cvInRangeS(imgHSV, cvScalar(10/2, 100, 100), cvScalar(25/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[3] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == BROWN){
			cvInRangeS(imgHSV, cvScalar(5/2, 100, 100), cvScalar(10/2, 250, 150), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[4] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == GREEN){
			cvInRangeS(imgHSV, cvScalar(140/2, 90,50), cvScalar(160/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[5] = cvGetCentralMoment(moments, 0, 0);
	   }


	}


	std::vector<int> temp;
	temp.push_back(-1);
	for(int x=0;x<6;x++){
		if(area[x]>500){
			temp.push_back(area[x]);
		}
	}


	while(temp.size() > 1){
		int temp2 = temp.back();
		temp.pop_back();
		int temp3 = temp.back();
		temp.pop_back();
		if(temp2 > temp3)
			temp.push_back(temp2);
		else
			temp.push_back(temp3);
	}

	for(int x=0;x<6;x++){
		if(area[x] == temp[0]){
			Mean[i][0]= x;
			if(area[x] < 500)
				Mean[i][1] = 0;
			if(area[x] > 500 && area[x] < 1800)
				Mean[i][1] = 1;
			if(area[x] > 1800)
				Mean[i][1] = 2;
			break;
		}
	}
	cvReleaseImage(&imgHSV);
	cvReleaseImage(&imgHSV2);

}

	int colorssort[6] = {0,0,0,0,0,0};
	int sizesort[3] = {0,0,0};

	for(int x=0;x<10;x++){
		if(Mean[x][0] == RED)
			colorssort[RED]++;
		if(Mean[x][0] == BLUE)
			colorssort[BLUE]++;
		if(Mean[x][0] == YELLOW)
			colorssort[YELLOW]++;
		if(Mean[x][0] == ORANGE)
			colorssort[ORANGE]++;
		if(Mean[x][0] == BROWN)
			colorssort[BROWN]++;
		if(Mean[x][0] == GREEN)
			colorssort[GREEN]++;
		if(Mean[x][1] == 0)
			sizesort[0]++;
		if(Mean[x][1] == 1)
			sizesort[1]++;
		if(Mean[x][1] == 2)
			sizesort[2]++;
	}

	std::vector<int> temp;
	temp.push_back(colorssort[0]);
	temp.push_back(colorssort[1]);
	temp.push_back(colorssort[2]);
	temp.push_back(colorssort[3]);
	temp.push_back(colorssort[4]);
	temp.push_back(colorssort[5]);
	while(temp.size() > 1){

			int temp2 = temp.back();
			temp.pop_back();
			int temp3 = temp.back();
			temp.pop_back();
			if(temp2 > temp3)
				temp.push_back(temp2);
			else
				temp.push_back(temp3);
	}
	for(int x=0;x<6;x++){
		if(colorssort[x] == temp[0]){
			Block[0]= x;
			break;
		}
	}
	std::vector<int> temp4;
	temp4.push_back(-1);
	temp4.push_back(sizesort[0]);
	temp4.push_back(sizesort[1]);
	temp4.push_back(sizesort[2]);
	while(temp4.size() > 1){
				int temp2 = temp4.back();
				temp4.pop_back();
				int temp3 = temp4.back();
				temp4.pop_back();
				if(temp2 > temp3)
					temp4.push_back(temp2);
				else
					temp4.push_back(temp3);
		}
	for(int x=0;x<3;x++){
			if(sizesort[x] == temp4[0]){
				Block[1]= x;
				break;
			}
		}
	return Block;

}
int* AutoRangeDistance(CvCapture* cap){
	IplImage* img = cvQueryFrame(cap);

	IplImage* imgHSV =  cvCreateImage(cvGetSize(img),8,3);
	IplImage* imgHSV2 = cvCreateImage(cvGetSize(img),8,1);


	cvCvtColor(img, imgHSV, CV_BGR2HSV);

	int color = 0;
	int area[6];
	int* Block =new int[6];

	for(int x=0;x<6;x++){
		Block[x] = -1;
	}
	CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));

	for(color=0;color<6;color++){

		if(color == RED){
			cvInRangeS(imgHSV, cvScalar(0/2, 100, 150), cvScalar(5/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[0] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == BLUE){
			cvInRangeS(imgHSV, cvScalar(210/2, 152, 0), cvScalar(215/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[1] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == YELLOW){
			cvInRangeS(imgHSV, cvScalar(30/2, 100, 100), cvScalar(60/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[2] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == ORANGE){
			cvInRangeS(imgHSV, cvScalar(10/2, 100, 100), cvScalar(25/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[3] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == BROWN){
			cvInRangeS(imgHSV, cvScalar(5/2, 100, 100), cvScalar(10/2, 250, 150), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[4] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == GREEN){
			cvInRangeS(imgHSV, cvScalar(140/2, 90,50), cvScalar(160/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[5] = cvGetCentralMoment(moments, 0, 0);
	   }


	}


	std::vector<int> temp;

	std::vector<int> temp3;

	int count = 7;
	bool brk = false;
	while(count > 0){
		--count;
	for(int x=0;x<6;x++){
		for(int y=0;y<temp3.size();y++)
			if(x == temp3[y]){
				brk = true;
				break;
			}
		if(brk){
			brk = false;
			continue;
		}
		temp.push_back(area[x]);
	}


	while(temp.size() > 1){
		int temp2 = temp.back();
		temp.pop_back();
		int temp3 = temp.back();
		temp.pop_back();
		if(temp2 > temp3)
			temp.push_back(temp2);
		else
			temp.push_back(temp3);
	}
	for(int x=0;x<6;x++){
			if(area[x] == temp[0]){
				temp3.push_back(x);
				break;
			}
	}


}
	for(int x=0;x<6;x++)
		Block[x] = temp3[x];

	cvReleaseImage(&imgHSV);
	cvReleaseImage(&imgHSV2);

	return Block;




}

int* AutoRangeDistanceAir(CvCapture* cap){
	IplImage* img = cvQueryFrame(cap);

	IplImage* imgHSV =  cvCreateImage(cvGetSize(img),8,3);
	IplImage* imgHSV2 = cvCreateImage(cvGetSize(img),8,1);



	cvCvtColor(img, imgHSV, CV_BGR2HSV);

	int color = 0;
	int area[6];
	int* Block =new int[2];
	Block[0] = -1;
	Block[1] = -1;
	CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));

	for(color=0;color<6;color++){

		if(color == RED){
			cvInRangeS(imgHSV, cvScalar(0/2, 100, 150), cvScalar(5/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[0] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == BLUE){
			cvInRangeS(imgHSV, cvScalar(210/2, 152, 0), cvScalar(215/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[1] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == YELLOW){
			cvInRangeS(imgHSV, cvScalar(30/2, 100, 100), cvScalar(60/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[2] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == ORANGE){
			cvInRangeS(imgHSV, cvScalar(10/2, 100, 100), cvScalar(25/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[3] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == BROWN){
			cvInRangeS(imgHSV, cvScalar(5/2, 100, 100), cvScalar(10/2, 250, 150), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[4] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == GREEN){
			cvInRangeS(imgHSV, cvScalar(140/2, 90,50), cvScalar(160/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[5] = cvGetCentralMoment(moments, 0, 0);
	   }


	}


	std::vector<int> temp;

	std::vector<int> temp3;

	int count = 2;
	bool brk = false;
	while(count > 0){
		--count;
	for(int x=0;x<6;x++){
		for(int y=0;y<temp3.size();y++)
			if(x == temp3[y]){
				brk = true;
				break;
			}
		if(brk){
			brk = false;
			continue;
		}
		temp.push_back(area[x]);
	}


	while(temp.size() > 1){
		int temp2 = temp.back();
		temp.pop_back();
		int temp3 = temp.back();
		temp.pop_back();
		if(temp2 > temp3)
			temp.push_back(temp2);
		else
			temp.push_back(temp3);
	}
	for(int x=0;x<6;x++){
			if(area[x] == temp[0]){
				temp3.push_back(x);
			}
	}


}
	for(int x=0;x<2;x++)
		Block[x] = temp3[x];

	cvReleaseImage(&imgHSV);
	cvReleaseImage(&imgHSV2);

	return Block;




}

bool AutoAlignTurret(CvCapture* cap,int color,Buffer* m_Buffer,int &degree ){
	IplImage* img = cvQueryFrame(cap);

	IplImage* imgHSV =  cvCreateImage(cvGetSize(img),8,3);
	IplImage* imgHSV2 = cvCreateImage(cvGetSize(img),8,1);

	double moments10;
	double moments01;
	double moments11;
	int posX = moments10/moments11;
	int posY = moments01/moments11;

	cvCvtColor(img, imgHSV, CV_BGR2HSV);
	CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));

	if(color == RED){
		cvInRangeS(imgHSV, cvScalar(0/2, 100, 150), cvScalar(5/2, 255, 255), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	}
	if(color == BLUE){
		cvInRangeS(imgHSV, cvScalar(210/2, 152, 0), cvScalar(215/2, 255, 255), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	}
	if(color == YELLOW){
		cvInRangeS(imgHSV, cvScalar(30/2, 100, 100), cvScalar(60/2, 255, 255), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	}
	if(color == ORANGE){
		cvInRangeS(imgHSV, cvScalar(10/2, 100, 100), cvScalar(25/2, 255, 255), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	  }
	if(color == BROWN){
		cvInRangeS(imgHSV, cvScalar(5/2, 100, 100), cvScalar(10/2, 250, 150), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	  }
	if(color == GREEN){
		cvInRangeS(imgHSV, cvScalar(140/2, 90,50), cvScalar(160/2, 255, 255), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	  }


	cvReleaseImage(&imgHSV);
	cvReleaseImage(&imgHSV2);
	if(img->width/2+10 > posX && img->width/2-10 < posX){
			return true;
		}
	else if(img->width/2 > posX){
		std::cout << "left" << std::endl;
		--degree;
		m_Buffer->writeBuffer("t",degree);
		usleep(500000);
		return false;
	}

	else if(img->width/2 < posX){
		std::cout << "right" << std::endl;
		degree++;
		m_Buffer->writeBuffer("t",degree);
		usleep(500000);
		return false;
	}
	else if(posX < 0 || posX > img->width){
			return false;
		}
	else
		return true;
}
bool AutoAlignChasey(CvCapture* cap,int color,Buffer* m_Buffer,int Direction,int &CposX){
	IplImage* img = cvQueryFrame(cap);

	IplImage* imgHSV =  cvCreateImage(cvGetSize(img),8,3);
	IplImage* imgHSV2 = cvCreateImage(cvGetSize(img),8,1);

	double moments10;
	double moments01;
	double moments11;
	int posX = moments10/moments11;
	int posY = moments01/moments11;

	cvCvtColor(img, imgHSV, CV_BGR2HSV);
	CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));

	if(color == RED){
		cvInRangeS(imgHSV, cvScalar(0/2, 100, 150), cvScalar(5/2, 255, 255), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	}
	if(color == BLUE){
		cvInRangeS(imgHSV, cvScalar(210/2, 152, 0), cvScalar(215/2, 255, 255), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	}
	if(color == YELLOW){
		cvInRangeS(imgHSV, cvScalar(30/2, 100, 100), cvScalar(60/2, 255, 255), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	}
	if(color == ORANGE){
		cvInRangeS(imgHSV, cvScalar(10/2, 100, 100), cvScalar(25/2, 255, 255), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	  }
	if(color == BROWN){
		cvInRangeS(imgHSV, cvScalar(5/2, 100, 100), cvScalar(10/2, 250, 150), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	  }
	if(color == GREEN){
		cvInRangeS(imgHSV, cvScalar(140/2, 90,50), cvScalar(160/2, 255, 255), imgHSV2);
		cvMoments(imgHSV2, moments, 1);
			moments10 = cvGetSpatialMoment(moments,1,0);
			moments01 = cvGetSpatialMoment(moments,0,1);
			moments11 = cvGetCentralMoment(moments,0,0);
			posX = moments10/moments11;
			posY = moments01/moments11;
	  }


	cvReleaseImage(&imgHSV);
	cvReleaseImage(&imgHSV2);

	if(img->width/2+8 > posX && img->width/2-8 < posX){
		return true;
	}
	else if(img->width/2+8 > posX){
		std::cout << "left" << std::endl;
		if(Direction == 0){
			m_Buffer->writeBuffer("f",1);
			CposX++;
		}
		if(Direction == 1){
			m_Buffer->writeBuffer("b",1);
			--CposX;
		}
		return false;
	}

	else if(img->width/-8 < posX){
		std::cout << "right" << std::endl;
		if(Direction == 0){
			m_Buffer->writeBuffer("b",1);
			--CposX;
		}
		if(Direction == 1){
			m_Buffer->writeBuffer("f",1);
			CposX++;
		}
		return false;
	}
	else if(posX < 0 || posX > img->width){
		return false;
	}
	else
		return true;
}

void AutoRangeDebug(CvCapture* cap){
	IplImage* img = cvQueryFrame(cap);
	IplImage* imgHSV =  cvCreateImage(cvGetSize(img),8,3);
	IplImage* imgHSV2 = cvCreateImage(cvGetSize(img),8,1);

	cvCvtColor(img, imgHSV, CV_BGR2HSV);

	int color = 0;
	double area[6];
	int* Block = new int[2];
	for(int x=0;x<6;x++)
		area[x] = 0;
	Block[0] = -1;
	Block[1] = -1;
	CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));

	for(color=0;color<6;color++){

		if(color == RED){
			cvInRangeS(imgHSV, cvScalar(0/2, 100, 150), cvScalar(5/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[0] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == BLUE){
			cvInRangeS(imgHSV, cvScalar(210/2, 152, 0), cvScalar(215/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[1] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == YELLOW){
			cvInRangeS(imgHSV, cvScalar(30/2, 100, 100), cvScalar(60/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[2] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == ORANGE){
			cvInRangeS(imgHSV, cvScalar(10/2, 100, 100), cvScalar(25/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[3] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == BROWN){
			cvInRangeS(imgHSV, cvScalar(5/2, 100, 100), cvScalar(10/2, 250, 150), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[4] = cvGetCentralMoment(moments, 0, 0);
	   }
		if(color == GREEN){
			cvInRangeS(imgHSV, cvScalar(140/2, 90,50), cvScalar(160/2, 255, 255), imgHSV2);
			cvMoments(imgHSV2, moments, 1);
			area[5] = cvGetCentralMoment(moments, 0, 0);
	   }


	}


	std::vector<int> temp;
	temp.push_back(-1);
	for(int x=0;x<6;x++){
		if(area[x]>500){
			temp.push_back(area[x]);
		}
	}


	while(temp.size() > 1){
		int temp2 = temp.back();
		temp.pop_back();
		int temp3 = temp.back();
		temp.pop_back();
		if(temp2 > temp3)
			temp.push_back(temp2);
		else
			temp.push_back(temp3);
	}

	for(int x=0;x<6;x++){
		if(area[x] == temp[0]){
			Block[0]= x;
			if(area[x] < 1000)
				Block[1] = 0;
			if(area[x] > 1000 && area[x] < 1500)
				Block[1] = 1;
			if(area[x] > 1500)
				Block[1] = 2;
			break;
		}
	}

	std::cout << "area: " << area[Block[0]] << std::endl;
	std::cout << "Color: " << Block[0] << std::endl;
	cvReleaseImage(&imgHSV);
	cvReleaseImage(&imgHSV2);

}


